
// import haxe.ds.Array;
// import oimo.common.Setting;
// import oimo.dynamics.TimeStep;
// import oimo.dynamics.constraint.*;
// import oimo.dynamics.constraint.info.JacobianRow;
// import oimo.dynamics.constraint.info.joint.*;
// import oimo.dynamics.constraint.joint.*;
// import oimo.dynamics.constraint.solver.common.JointSolverMassDataRow;
// import oimo.m.Mat3;
// import oimo.m.Vec3;
// import oimo.m.M;

import { ConstraintSolver } from "../../ConstraintSolver";
import { JointSolverInfo } from "../../info/joint/JointSolverInfo";
import { Setting } from "../../../../common/Setting";
import { TimeStep } from "../../../TimeStep";
import { Mat3 } from "../../../../common/Mat3";
import { M } from "../../../../common/M";
import { JointSolverInfoRow } from "../../info/joint/JointSolverInfoRow";
import { JacobianRow } from "../../info/JacobianRow";
import { Vec3 } from "../../../../common/Vec3";
import { JointImpulse } from "../../joint/JointImpulse";
import { Joint } from "../../joint/Joint";
import { JointSolverMassDataRow } from "../common/JointSolverMassDataRow";

/**
 * A joint constraint solver using projected Gauss-Seidel (sequential impulse).
 */

export class PgsJointConstraintSolver extends ConstraintSolver {
	joint: Joint;
	info: JointSolverInfo;
	massData: Array<JointSolverMassDataRow>;


	constructor(joint: Joint) {
		super();
		this.joint = joint;

		this.info = new JointSolverInfo();

		this.massData = new Array<JointSolverMassDataRow>(Setting.maxJacobianRows);
		for (let i = 0; i < this.massData.length; ++i) {
			this.massData[i] = new JointSolverMassDataRow();
		}
	}

	public preSolveVelocity(timeStep: TimeStep): void {
		this.joint._syncAnchors();
		this.joint._getVelocitySolverInfo(timeStep, this.info);

		this._b1 = this.info.b1;
		this._b2 = this.info.b2;

		var invM1: number = this._b1._invMass;
		var invM2: number = this._b2._invMass;

		var invI1: Mat3 = this._b1._invInertia.clone();
		var invI2: Mat3 = this._b2._invInertia.clone();

		// compute mass data
		for (let i = 0; i < this.info.numRows; ++i) {
			var row: JointSolverInfoRow = this.info.rows[i];
			var md: JointSolverMassDataRow = this.massData[i];
			var j: JacobianRow = row.jacobian;

			j.updateSparsity();

			if (j.isLinearSet()) {
				M.vec3_scale(md.invMLin1, j.lin1, invM1);
				M.vec3_scale(md.invMLin2, j.lin2, invM2);
			} else {
				md.invMLin1.zero();
				md.invMLin2.zero();
			}

			if (j.isAngularSet()) {
				M.vec3_mulMat3(md.invMAng1, j.ang1, invI1);
				M.vec3_mulMat3(md.invMAng2, j.ang2, invI2);
			} else {
				md.invMAng1.zero();
				md.invMAng2.zero();
			}

			md.massWithoutCfm =
				M.vec3_dot(md.invMLin1, j.lin1) +
				M.vec3_dot(md.invMLin2, j.lin2) +
				M.vec3_dot(md.invMAng1, j.ang1) +
				M.vec3_dot(md.invMAng2, j.ang2)
				;
			md.mass = md.massWithoutCfm + row.cfm;

			if (md.massWithoutCfm != 0) md.massWithoutCfm = 1 / md.massWithoutCfm;
			if (md.mass != 0) md.mass = 1 / md.mass;
		}
	}

	public warmStart(timeStep: TimeStep): void {

		var factor: number = this.joint._getWarmStartingFactor();

		// adjust impulse for variable time step
		factor *= timeStep.dtRatio;

		// warm start disabled
		if (factor <= 0) {
			for (let i = 0; i < this.info.numRows; ++i) {
				var row: JointSolverInfoRow = this.info.rows[i];
				row.impulse.clear();
			}
			return;
		}

		var lv1: Vec3 = this._b1._vel.clone();
		var lv2: Vec3 = this._b2._vel.clone();
		var av1: Vec3 = this._b1._angVel.clone();
		var av2: Vec3 = this._b2._angVel.clone();

		for (let i = 0; i < this.info.numRows; ++i) {
			var row: JointSolverInfoRow = this.info.rows[i];
			var md: JointSolverMassDataRow = this.massData[i];
			var imp: JointImpulse = row.impulse;
			var j: JacobianRow = row.jacobian;

			imp.impulse *= factor;
			imp.impulseM *= factor;

			var impulse: number = imp.impulse + imp.impulseM;

			// apply initial impulse
			M.vec3_addRhsScaledEQ(lv1, lv1, md.invMLin1, impulse);
			M.vec3_addRhsScaledEQ(lv2, lv2, md.invMLin2, -impulse);
			M.vec3_addRhsScaledEQ(av1, av1, md.invMAng1, impulse);
			M.vec3_addRhsScaledEQ(av2, av2, md.invMAng2, -impulse);
		}

		M.vec3_assign(this._b1._vel, lv1);
		M.vec3_assign(this._b2._vel, lv2);
		M.vec3_assign(this._b1._angVel, av1);
		M.vec3_assign(this._b2._angVel, av2);
	}

	public solveVelocity(): void {
		var lv1: Vec3 = this._b1._vel.clone();
		var lv2: Vec3 = this._b2._vel.clone();
		var av1: Vec3 = this._b1._angVel.clone();
		var av2: Vec3 = this._b2._angVel.clone();

		// solve motor
		for (let i = 0; i < this.info.numRows; ++i) {
			var row: JointSolverInfoRow = this.info.rows[i];
			var md: JointSolverMassDataRow = this.massData[i];
			var imp: JointImpulse = row.impulse;
			var j: JacobianRow = row.jacobian;

			if (row.motorMaxImpulse == 0) continue;

			// measure relative velocity
			var rv: number = 0;
			rv += M.vec3_dot(lv1, j.lin1);
			rv -= M.vec3_dot(lv2, j.lin2);
			rv += M.vec3_dot(av1, j.ang1);
			rv -= M.vec3_dot(av2, j.ang2);

			var impulseM: number = (-row.motorSpeed - rv) * md.massWithoutCfm;

			// clamp impulse
			var oldImpulseM: number = imp.impulseM;
			imp.impulseM += impulseM;
			if (imp.impulseM < -row.motorMaxImpulse) {
				imp.impulseM = -row.motorMaxImpulse;
			} else if (imp.impulseM > row.motorMaxImpulse) {
				imp.impulseM = row.motorMaxImpulse;
			}
			impulseM = imp.impulseM - oldImpulseM;

			// apply delta impulse
			if (j.isLinearSet()) {
				M.vec3_addRhsScaledEQ(lv1, lv1, md.invMLin1, impulseM);
				M.vec3_addRhsScaledEQ(lv2, lv2, md.invMLin2, -impulseM);
			}
			if (j.isAngularSet()) {
				M.vec3_addRhsScaledEQ(av1, av1, md.invMAng1, impulseM);
				M.vec3_addRhsScaledEQ(av2, av2, md.invMAng2, -impulseM);
			}
		}

		// solve normal
		for (let i = 0; i < this.info.numRows; ++i) {
			var row: JointSolverInfoRow = this.info.rows[i];
			var md: JointSolverMassDataRow = this.massData[i];
			var imp: JointImpulse = row.impulse;
			var j: JacobianRow = row.jacobian;

			// measure relative velocity
			var rv: number = 0;
			rv += M.vec3_dot(lv1, j.lin1);
			rv -= M.vec3_dot(lv2, j.lin2);
			rv += M.vec3_dot(av1, j.ang1);
			rv -= M.vec3_dot(av2, j.ang2);

			var impulse: number = (row.rhs - rv - imp.impulse * row.cfm) * md.mass;

			// clamp impulse
			var oldImpulse: number = imp.impulse;
			imp.impulse += impulse;
			if (imp.impulse < row.minImpulse) {
				imp.impulse = row.minImpulse;
			} else if (imp.impulse > row.maxImpulse) {
				imp.impulse = row.maxImpulse;
			}
			impulse = imp.impulse - oldImpulse;

			// apply delta impulse
			if (j.isLinearSet()) {
				M.vec3_addRhsScaledEQ(lv1, lv1, md.invMLin1, impulse);
				M.vec3_addRhsScaledEQ(lv2, lv2, md.invMLin2, -impulse);
			}
			if (j.isAngularSet()) {
				M.vec3_addRhsScaledEQ(av1, av1, md.invMAng1, impulse);
				M.vec3_addRhsScaledEQ(av2, av2, md.invMAng2, -impulse);
			}
		}

		M.vec3_assign(this._b1._vel, lv1);
		M.vec3_assign(this._b2._vel, lv2);
		M.vec3_assign(this._b1._angVel, av1);
		M.vec3_assign(this._b2._angVel, av2);
	}

	public postSolveVelocity(timeStep: TimeStep): void {
		// compute total linear and angular impulse
		var lin: Vec3 = new Vec3();
		var ang: Vec3 = new Vec3();

		for (let i = 0; i < this.info.numRows; ++i) {
			var row: JointSolverInfoRow = this.info.rows[i];
			var imp: JointImpulse = row.impulse;
			var j: JacobianRow = row.jacobian;
			if (j.isLinearSet()) {
				// assume that this row is linear
				M.vec3_addRhsScaledEQ(lin, lin, j.lin1, imp.impulse);
			} else if (j.isAngularSet()) {
				// assume that this row is angular
				M.vec3_addRhsScaledEQ(ang, ang, j.ang1, imp.impulse);
			}
		}

		M.vec3_scale(this.joint._appliedForce, lin, timeStep.invDt);
		M.vec3_scale(this.joint._appliedTorque, ang, timeStep.invDt);
	}


	private updatePositionData(): void {
		this.joint._syncAnchors();
		this.joint._getPositionSolverInfo(this.info);

		this._b1 = this.info.b1;
		this._b2 = this.info.b2;

		var invM1: number = this._b1._invMass;
		var invM2: number = this._b2._invMass;

		var invI1: Mat3 = this._b1._invInertia.clone();
		var invI2: Mat3 = this._b2._invInertia.clone();

		// compute mass data
		for (let i = 0; i < this.info.numRows; ++i) {
			var row: JointSolverInfoRow = this.info.rows[i];
			var md: JointSolverMassDataRow = this.massData[i];
			var imp: JointImpulse = row.impulse;
			var j: JacobianRow = row.jacobian;

			M.vec3_scale(md.invMLin1, j.lin1, invM1);
			M.vec3_scale(md.invMLin2, j.lin2, invM2);
			M.vec3_mulMat3(md.invMAng1, j.ang1, invI1);
			M.vec3_mulMat3(md.invMAng2, j.ang2, invI2);

			md.mass =
				M.vec3_dot(md.invMLin1, j.lin1) +
				M.vec3_dot(md.invMLin2, j.lin2) +
				M.vec3_dot(md.invMAng1, j.ang1) +
				M.vec3_dot(md.invMAng2, j.ang2)
				;

			if (md.mass != 0) md.mass = 1 / md.mass;
		}
	}

	public preSolvePosition(timeStep: TimeStep): void {
		this.updatePositionData();

		// clear position impulses
		for (let i = 0; i < this.info.numRows; ++i) {
			this.info.rows[i].impulse.impulseP = 0;
		}
	}

	public solvePositionSplitImpulse(): void {
		var lv1: Vec3 = this._b1._pseudoVel.clone();
		var lv2: Vec3 = this._b2._pseudoVel.clone();
		var av1: Vec3 = this._b1._angPseudoVel.clone();
		var av2: Vec3 = this._b2._angPseudoVel.clone();

		for (let i = 0; i < this.info.numRows; ++i) {
			var row: JointSolverInfoRow = this.info.rows[i];
			var md: JointSolverMassDataRow = this.massData[i];
			var imp: JointImpulse = row.impulse;
			var j: JacobianRow = row.jacobian;

			// measure relative velocity
			var rv: number = 0;
			rv += M.vec3_dot(lv1, j.lin1);
			rv -= M.vec3_dot(lv2, j.lin2);
			rv += M.vec3_dot(av1, j.ang1);
			rv -= M.vec3_dot(av2, j.ang2);

			var impulseP: number = (row.rhs * Setting.positionSplitImpulseBaumgarte - rv) * md.mass;

			// clamp impulse
			var oldImpulseP: number = imp.impulseP;
			imp.impulseP += impulseP;
			if (imp.impulseP < row.minImpulse) {
				imp.impulseP = row.minImpulse;
			} else if (imp.impulseP > row.maxImpulse) {
				imp.impulseP = row.maxImpulse;
			}

			impulseP = imp.impulseP - oldImpulseP;

			// apply delta impulse
			M.vec3_addRhsScaledEQ(lv1, lv1, md.invMLin1, impulseP);
			M.vec3_addRhsScaledEQ(lv2, lv2, md.invMLin2, -impulseP);
			M.vec3_addRhsScaledEQ(av1, av1, md.invMAng1, impulseP);
			M.vec3_addRhsScaledEQ(av2, av2, md.invMAng2, -impulseP);
		}

		M.vec3_assign(this._b1._pseudoVel, lv1);
		M.vec3_assign(this._b2._pseudoVel, lv2);
		M.vec3_assign(this._b1._angPseudoVel, av1);
		M.vec3_assign(this._b2._angPseudoVel, av2);
	}

	public solvePositionNgs(timeStep: TimeStep): void {
		this.updatePositionData();

		var lv1: Vec3 = new Vec3();
		var lv2: Vec3 = new Vec3();
		var av1: Vec3 = new Vec3();
		var av2: Vec3 = new Vec3();

		for (let i = 0; i < this.info.numRows; ++i) {
			var row: JointSolverInfoRow = this.info.rows[i];
			var md: JointSolverMassDataRow = this.massData[i];
			var imp: JointImpulse = row.impulse;
			var j: JacobianRow = row.jacobian;

			// measure relative velocity
			var rv: number = 0;
			rv += M.vec3_dot(lv1, j.lin1);
			rv -= M.vec3_dot(lv2, j.lin2);
			rv += M.vec3_dot(av1, j.ang1);
			rv -= M.vec3_dot(av2, j.ang2);

			var impulseP: number = (row.rhs * Setting.positionNgsBaumgarte - rv) * md.mass;

			// clamp impulse
			var oldImpulseP: number = imp.impulseP;
			imp.impulseP += impulseP;
			if (imp.impulseP < row.minImpulse) {
				imp.impulseP = row.minImpulse;
			} else if (imp.impulseP > row.maxImpulse) {
				imp.impulseP = row.maxImpulse;
			}

			impulseP = imp.impulseP - oldImpulseP;

			// apply delta impulse
			M.vec3_addRhsScaledEQ(lv1, lv1, md.invMLin1, impulseP);
			M.vec3_addRhsScaledEQ(lv2, lv2, md.invMLin2, -impulseP);
			M.vec3_addRhsScaledEQ(av1, av1, md.invMAng1, impulseP);
			M.vec3_addRhsScaledEQ(av2, av2, md.invMAng2, -impulseP);
		}

		(this._b1._applyTranslation(lv1));
		(this._b2._applyTranslation(lv2));
		(this._b1._applyRotation(av1));
		(this._b2._applyRotation(av2));
	}

	public postSolve(): void {
		this.joint._syncAnchors();
		this.joint._checkDestruction();
	}

}
